#include <stdlib.h>

#include "main.h"
#include "usart.h"

#include "openmv.h"
#include "calculate.h"
#include "arm_math.h"

MeanFilter openmv_rho_num_maen;
MeanFilter openmv_theta_num_maen;

void openmv_init()
{
	Filter_MeanInit(&openmv_rho_num_maen, 5);
	Filter_MeanInit(&openmv_theta_num_maen, 5);
}


uint8_t openmv_data[17];

uint8_t openmv_rho_str[5];
uint8_t openmv_theta_str[4];
uint8_t openmv_magnitude_str[4];

int16_t openmv_rho_num;
int16_t openmv_theta_num;
int16_t openmv_magnitude_num;

void find_openmv_str()
{
	uint8_t inspect_code[5] = "JLDS";
	for(uint8_t i = 0;i<17;i++)
	{
		uint8_t j=i;
		if(openmv_data[j]==inspect_code[0])
		{
			j++;
			j = (j>16)?(j-17):(j);
			if(openmv_data[j]==inspect_code[1])
			{
				j++;
				j = (j>16)?(j-17):(j);
				if(openmv_data[j]==inspect_code[2])
				{
					j++;
					j = (j>16)?(j-17):(j);
					if(openmv_data[j]==inspect_code[3])
					{
						j++;
						j = (j>16)?(j-17):(j);
						openmv_rho_str[0] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_rho_str[1] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_rho_str[2] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_rho_str[3] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_theta_str[0] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_theta_str[1] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_theta_str[2] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_magnitude_str[0] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_magnitude_str[1] = openmv_data[j];
						j++;
						j = (j>16)?(j-17):(j);
						openmv_magnitude_str[2] = openmv_data[j];
						
						openmv_rho_num=atoi((char*)openmv_rho_str);
						openmv_theta_num=atoi((char*)openmv_theta_str);
						openmv_magnitude_num=atoi((char*)openmv_magnitude_str);
						
						analysis_openmv_data();
					}
				}
			}
		}
	}
}
//float x_target,y_target,w_target;
//float xiangsu2mm=0.667,car_half_mm=140;
//float center2line;
//float angle,sin_angle,cos_angle;
//void analysis_openmv_data()
//{
//	if(openmv_theta_num!=0 && openmv_theta_num!=180 && openmv_theta_num!=90)
//	{
//		angle = openmv_theta_num * (3.14f/180.f);
//		sin_angle = arm_sin_f32(angle);
//		cos_angle = arm_cos_f32(angle);
//		center2line = (((80.f-openmv_rho_num/cos_angle)*(cos_angle/sin_angle)+120.f)*xiangsu2mm+car_half_mm)*sin_angle;
//		if(openmv_theta_num<90)
//		{
//			x_target = center2line * sin_angle;
//			y_target = -center2line * cos_angle;
//			w_target = openmv_theta_num;
//		}
//		else if(openmv_theta_num>90)
//		{
//			x_target = center2line * sin_angle;
//			y_target = center2line * -cos_angle;
//			w_target = openmv_theta_num-180;
//		}
//	}
//	else
//	{
//		x_target = 0;
//		y_target = (80-ABS(openmv_rho_num))*xiangsu2mm;
//		w_target = 0;
//	}
//}

float x_target,y_target,w_target;
float angle,sin_angle,cos_angle,tan_angle;
void analysis_openmv_data()
{
	if(openmv_theta_num!=0 && openmv_theta_num!=180 && openmv_theta_num!=90)
	{
		angle = openmv_theta_num * (3.14f/180.f);
		sin_angle = arm_sin_f32(angle);
		cos_angle = arm_cos_f32(angle);
		tan_angle = sin_angle/cos_angle;
		y_target = openmv_rho_num/cos_angle - 80 - 60*tan_angle;
		if(openmv_theta_num<90)
		{
			w_target = openmv_theta_num;
		}
		else if(openmv_theta_num>90)
		{
			w_target = openmv_theta_num-180;
		}
	}
	else
	{
		y_target = ABS(openmv_rho_num)-80;
		w_target = 0;
	}
}





